#include "pid_t.h"
#include "string.h"
#include "pid_pro.h"
/************************************************
本程序仅供学习，引用代码请标明出处
使用教程：https://blog.csdn.net/loop222/article/details/120471390
创建日期：20210925
作    者：loop222 @郑州
************************************************/
/******************************************************************************/


pid_struct_t motor_pid ;
pid_struct_t motor_dule_loop[2];
/******************************************************************************/
//void PID_init(void)
//{
//	memcpy(&motor_pid,&motor_pid_t,sizeof (pid_struct_t));
//	memcpy(&motor_dule_loop[0],&motor_pid_dule_loop_speed_t,sizeof(pid_struct_t));
//	memcpy(&motor_dule_loop[1],&motor_pid_dule_loop_angle_t,sizeof(pid_struct_t));
//	
//}
/******************************************************************************/
//just P&I is enough,no need D 
float PID_cal(pid_struct_t* pid ,float ref , float cur)
{
  float output;
  pid->err[1] = pid->err[0];
  pid->err[0] = ref - cur;
  
  pid->p_out  = pid->kp * pid->err[0];
  pid->i_out += pid->ki * pid->err[0];
  pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
  
  output = pid->p_out + pid->i_out + pid->d_out;
  LIMIT_MIN_MAX(output, -pid->out_max, pid->out_max);
  return output;
}
